/**
  ******************************************************************************
  * Copyright (c) 2022 - ~, SCUT-RobotLab Development Team
  * @file    Hardware_Node.h
  * @author   
  * @brief   
  ******************************************************************************
  */
#ifndef JPOSINITIALIZER_H
#define JPOSINITIALIZER_H

/* Includes ------------------------------------------------------------------*/
#include "common/Controllers/LegController.h"
#include "common/Dynamics/Quadruped.h"
/* Private macros ------------------------------------------------------------*/
/* Private type --------------------------------------------------------------*/
/* Exported function declarations --------------------------------------------*/	


/*!
 * Controller to initialize the position of the legs on power-on
 */
template <typename T>
class JPosInitializer {
 public:
  JPosInitializer(T end_time, float dt);
  ~JPosInitializer();

  bool IsInitialized(LegController<T>*);

 private:
  void _UpdateParam();
  void _UpdateInitial(LegController<T>* ctrl);
  bool _b_first_visit;
  T _end_time;
  T _curr_time;
  T _dt;

  std::vector<T> _ini_jpos;
  std::vector<T> _target_jpos;
  std::vector<T> _mid_jpos;

  // BS_Basic<T, cheetah::num_act_joint, 3, 1, 2, 2> _jpos_trj;
};

#endif
/************************ COPYRIGHT(C) SCUT-ROBOTLAB **************************/
 

 

